
#include <Servo.h>

Servo mLeft;
Servo mRight;
Servo mGrab;


const int ARight1 = A1;
const int ARight2 = A0;


const int ACenterRight = A2;
const int ACenterLeft = A3;

const int ALeft1 = A4;
const int ALeft2 = A5;



const int photoresistor = A6;

const int hikari = A7;

const int LEDCenter = 28;
const int LEDRight = 30;
const int LEDLeft = 26;

const int LEDWhite = 32;
const int LEDBlack = 34;

const int LEDRed = 36;
const int LEDGreen = 38;
const int LEDBlue = 40;


const int StartB = 21;
const int StopB = 20;

const int colorB = 42;

const int b1 = 41 ;

const int UEcho = 5;
const int UTrig = 4;


int maxhikari = 0;
int hikarimichi=0;

int grabbed = 0;

int stopped = 1;

int startF = 0;

int IRLeft1 = 0;
int IRLeft2 = 0;
int IRRight1 = 0;
int IRRight2 = 0;
int IRCenterLeft = 0;
int IRCenterRight = 0;


int mspeed = 31;
int turnspeed = 0;

int turnspeedCenter = 15;  //20
int turnspeedSide1 = 25; // 25
int turnspeedSide2 = 28;  // 28

int lastdirection=-1; 


int Amax = 150;
int AmaxCenter = 150;
int AmaxSide1 = 150;
int AmaxSide2 = 150;

int Ahikari =0;

int Atimes = 1;

int outtimes = 0;
int Ttimes=0;

int UDistance = 0;

// Define colour sensor LED pins
//int ledArray[] = {48,50,52};
int ledArray[] = {48,50};

// boolean to know if the balance has been set
boolean balanceSet = false;

int IRcalibration = 0;

//place holders for colour detected
int red = 0;
int green = 0;
int blue = 0;

//floats to hold colour arrays
float colourArray[] = {0,0};
float whiteArray[] = {0,0};
float blackArray[] = {0,0};


int colorcycle=0;
int cycletimes=0;
int colorReading=0;
int colorReady=0;
const int readingtimes=6;

int firstColorSet=1;


//place holder for average
int avgRead;


void setup() {

 pinMode(8, OUTPUT);
 pinMode(7, OUTPUT);
 pinMode(6, OUTPUT);
 
   
digitalWrite(6, LOW);
digitalWrite(7, LOW);
digitalWrite(8, LOW);


  mGrab.attach(8);
  mLeft.attach(7);
  mRight.attach(6);
  
  pinMode(48,OUTPUT);
  pinMode(50,OUTPUT);
  pinMode(52,OUTPUT);
  
  pinMode(LEDCenter, OUTPUT);
  pinMode(LEDLeft, OUTPUT);
  pinMode(LEDRight, OUTPUT);
  pinMode(LEDWhite, OUTPUT);
  pinMode(LEDBlack, OUTPUT);
  pinMode(LEDRed, OUTPUT);
  pinMode(LEDGreen, OUTPUT);
  pinMode(LEDBlue, OUTPUT);
  
 pinMode(UTrig, OUTPUT);
 pinMode(UEcho, INPUT);
  
  pinMode(colorB, INPUT);
  pinMode(b1, INPUT);
  
  digitalWrite(LEDCenter, LOW);
  digitalWrite(LEDRight, LOW);
  digitalWrite(LEDLeft, LOW);
  
  digitalWrite(LEDWhite, LOW);
  digitalWrite(LEDBlack, LOW);
  digitalWrite(LEDRed, LOW);
  digitalWrite(LEDGreen, LOW);
  digitalWrite(LEDBlue, LOW);
  
  pinMode(StartB, INPUT); 
  pinMode(StopB, INPUT); 
  
  attachInterrupt(2, startM, LOW);
  attachInterrupt(3, stopM, LOW);

  Serial.begin(9600); 
  
  mLeft.write(90); 
  mRight.write(90);
  mGrab.write(90);

  

}


void IRcalib(){
  
  int val1;
  int val2;
  

        
     IRLeft1 = readIR(ALeft1);
     IRLeft2 = readIR(ALeft2);
     IRRight1 = readIR(ARight1);
     IRRight2 = readIR(ARight2);
     IRCenterLeft = readIR(ACenterLeft);
     IRCenterRight = readIR(ACenterRight);

     AmaxCenter = (IRCenterLeft+IRCenterRight)/2 + 10;
     
     AmaxSide1 = (IRLeft1 + IRRight1)/2 + 10;
     AmaxSide2 = (IRLeft2 + IRRight2)/2 + 300;

  
 mLeft.write(90); 
 mRight.write(90);
  
 
 
 IRcalibration=1;
  
}


int readIR(int a){
 
  int sum = 0;
  int r =0;
  
 for(int i = 0; i<Atimes ; i++){
    r = analogRead(a);
    delay(10); 
    sum = sum + r;
 } 
  
  return sum/Atimes ;
  
}

void startM(){
  
  
  startF = 1;
  delay(100);
  
}

void stopM(){
  
  startF = 0;
  delay(100);
  
}




void checkBalance(){
  //check if the balance has been set, if not, set it
  if(balanceSet == false){
    setBalance();
  }
}

void setBalance(){
  //set white balance
   delay(5000);                              //delay for five seconds, this gives us time to get a white sample in front of our sensor
  //scan the white sample.
  //go through each light, get a reading, set the base reading for each colour red, green, and blue to the white array
  for(int i = 0;i<=1;i++){
     digitalWrite(ledArray[i],HIGH);
     delay(100);
     getReading(5);          //number is the number of scans to take for average, this whole function is redundant, one reading works just as well.
     whiteArray[i] = avgRead;
     digitalWrite(ledArray[i],LOW);
     delay(100);
  }
  
    Serial.print("White : "+ String(int(whiteArray[0]))+" "+String(int(whiteArray[1]))+"\n");
    
  //done scanning white, now it will pulse blue to tell you that it is time for the black (or grey) sample.
   //set black balance
    delay(5000);              //wait for five seconds so we can position our black sample 
  //go ahead and scan, sets the colour values for red, green, and blue when exposed to black
  for(int i = 0;i<=1;i++){
     digitalWrite(ledArray[i],HIGH);
     delay(100);
     getReading(5);
     blackArray[i] = avgRead;
     //blackArray[i] = analogRead(2);
     digitalWrite(ledArray[i],LOW);
     delay(100);
  }
  
  
  

   Serial.print("Black : "+ String(int(blackArray[0]))+" "+String(int(blackArray[1]))+"\n");
   
   delay(5000);

   //set boolean value so we know that balance is set
  balanceSet = true;
  //delay another 5 seconds to allow the human to catch up to what is going on

  }


int checkColour(){
  
       if(balanceSet == false){
        return -1;
      }
      
      
  

     digitalWrite(ledArray[colorcycle],HIGH);

     //delay(10);
     
     if(firstColorSet==1){
        
        firstColorSet=0;
        return 0;
        
      }
     
     int ph = analogRead(photoresistor);
     
     colorReading =ph + colorReading;
     
    Serial.print("colorReading : "+ String(ph)+"\n");
     
   
     if(++cycletimes >= readingtimes){
       
         
         colourArray[colorcycle] = colorReading/readingtimes;
         float greyDiff = whiteArray[colorcycle] - blackArray[colorcycle];
         colourArray[colorcycle] = (colourArray[colorcycle] - blackArray[colorcycle])/(greyDiff)*255;
         digitalWrite(ledArray[colorcycle],LOW);
       
         colorReading=0;

       
         if(++colorcycle >= 2){
       
             colorcycle=0;
             colorReady=1;
             
          }
       
       cycletimes=0;
       firstColorSet=1;
       return 1;
     }
       
       
  return 0;
}




void getReading(int times){
  int reading;
  int tally=0;
  //take the reading however many times was requested and add them up
for(int i = 0;i < times;i++){
   reading = analogRead(photoresistor);
   tally = reading + tally;
   delay(10);
}
//calculate the average and set it
avgRead = (tally)/times;
}


int  ledcolor(){
  
        digitalWrite(LEDWhite, LOW);
        digitalWrite(LEDBlack, LOW);
        digitalWrite(LEDRed, LOW);
        digitalWrite(LEDGreen, LOW);
        digitalWrite(LEDBlue, LOW);
  
  if(balanceSet == false){
    return -1;
  }
  
  //checkColour();
  printColour();
        

        
        if(colourArray[0] > 150 && colourArray[1] < 80){
          
            digitalWrite(LEDRed, HIGH);
            
            

             
                  mLeft.write(100);
                  mRight.write(100);
                  
                  delay(400);
                        
                   while (IRCenterLeft < AmaxCenter){
                     IRCenterLeft = analogRead(ACenterLeft);  
                     delay(20);                           
                  }
                  
          
              
          return 2;
          
        }
        
        if(colourArray[1] > 100 && colourArray[0] < 90){
          
          digitalWrite(LEDGreen, HIGH);
          
              mLeft.write(90);
              mRight.write(90);
              
              
                
                  int count = 25;
                  
                   while(count <=90){
                       count = count +2;
                       mGrab.write(count); 
                       delay(10);
                  }
    
    
                  
                  
                  grabbed=0;
              
                  startF=0;
            

              
          return 3;
          
        }
        

        
  return -1;
}

void colorCalib(){
  
        digitalWrite(LEDWhite, LOW);
        digitalWrite(LEDBlack, LOW);
        digitalWrite(LEDRed, LOW);
        digitalWrite(LEDGreen, LOW);
        digitalWrite(LEDBlue, LOW);
  
        digitalWrite(LEDLeft, HIGH);
        digitalWrite(LEDCenter, HIGH);
        digitalWrite(LEDRight,HIGH);
        
        delay(200);
        
        checkBalance();
        checkColour();
        printColour();

  
}


void printColour(){
    Serial.print("R = ");
    Serial.println(int(colourArray[0]));
    Serial.print("G = ");
    Serial.println(int(colourArray[1]));

    //delay(2000);
}



void Uget(){
  
      long  duration;
      
      digitalWrite(UTrig, LOW); 
      delayMicroseconds(2); 
    
     digitalWrite(UTrig, HIGH);
     delayMicroseconds(10); 
     
     digitalWrite(UTrig, LOW);
     duration = pulseIn(UEcho, HIGH);
     
     //Calculate the distance (in cm) based on the speed of sound.
      UDistance = duration/58.2;

}



void objectDetected(){
  
  
 
    mLeft.write(90);
    mRight.write(90);
    
    delay(100);
    
    while(1){
      
      
     IRCenterLeft = readIR(ACenterLeft);
     IRCenterRight = readIR(ACenterRight);
    
    if(IRCenterLeft > AmaxCenter &&  IRCenterRight < AmaxCenter){

        mLeft.write(92);
        mRight.write(92);

    }
    
    
    if(IRCenterLeft < AmaxCenter &&  IRCenterRight > AmaxCenter){
  
        mLeft.write(88);
        mRight.write(88);
                   
    }
      
      
     if(IRCenterLeft < AmaxCenter +10  &&  IRCenterRight < AmaxCenter +10){
       
        mLeft.write(90);
        mRight.write(90);
        
        break;
       
     }
      
    }

    
    while(UDistance > 2){
      
      Uget(); 
      IRCenterLeft = readIR(ACenterLeft);
      IRCenterRight = readIR(ACenterRight);
      
      Serial.print(String(UDistance)+" "+"\n" );
      
      if(IRCenterLeft > AmaxCenter &&  IRCenterRight < AmaxCenter){

        mLeft.write(92);
        mRight.write(90);
        continue;

      }
    
    
      if(IRCenterLeft < AmaxCenter &&  IRCenterRight > AmaxCenter){
  
        mLeft.write(90);
        mRight.write(88);
        continue;
                   
      }
      
      
      mLeft.write(92);
      mRight.write(88);
      delay(50);  
      
   
     
  
      
      
    }
    
    int count = 90;
    
    while(count >25){
     count = count -2;
     mGrab.write(count); 
     delay(10);
    }
    
    
    grabbed=1;
    
    //startF=0; 
  
}



void loop() {
 
  
 if(startF == 0){
    digitalWrite(LEDWhite, HIGH);
    digitalWrite(LEDBlack, HIGH);
    digitalWrite(LEDRed, HIGH);
    digitalWrite(LEDGreen, HIGH);
    digitalWrite(LEDBlue, HIGH);
    
    
    if (stopped == 0){
    
      mLeft.write(90);
      mRight.write(90); 
      mGrab.write(90);
      
      stopped=1;
    }
    
    
    if (digitalRead(colorB) == LOW){
  
        digitalWrite(LEDLeft, HIGH);
        digitalWrite(LEDCenter, HIGH);
        digitalWrite(LEDRight,HIGH);
        checkBalance();
    }
    
    return;
   }
  
  stopped =0;

  
   
  if(IRcalibration==0){
     IRcalib();
  }
  
  
 
  
  
  Uget();
  
  IRLeft1 = readIR(ALeft1);
  IRLeft2 = readIR(ALeft2);
  IRRight1 = readIR(ARight1);
  IRRight2 = readIR(ARight2);
  IRCenterLeft = readIR(ACenterLeft);
  IRCenterRight = readIR(ACenterRight);
  Ahikari = analogRead(hikari);
  
  
  
    Serial.print(String(IRLeft2)+" "+ 
    String(IRLeft1)+" "+
    String(IRCenterLeft)+" "+ 
    String(IRCenterRight)+" "+ 
    String(IRRight1)+" "+
    String(IRRight2)+" "+
    String(UDistance)+" "+
    String(Ahikari)+" "+"\n" ); 

  int flag=0;
  
    
  if(UDistance < 13 && grabbed == 0){
    
    objectDetected();

  }
  
  
   checkColour();
  
  if(colorReady==1){
     
     ledcolor();
   
     colorReady=0;
  }else{
       
      digitalWrite(LEDWhite, LOW);
      digitalWrite(LEDBlack, LOW);
      digitalWrite(LEDRed, LOW);
      digitalWrite(LEDGreen, LOW);
      digitalWrite(LEDBlue, LOW);  
    
  }
  
  
  if(IRLeft2 < AmaxSide2 && IRLeft1 < AmaxSide1 && IRCenterLeft < AmaxCenter && IRCenterRight < AmaxCenter && IRRight1 < AmaxSide1 && IRRight2 < AmaxSide2){
    
   
             if(outtimes < 10){
                  outtimes++;
                  return;
              }
          
                
              outtimes=0;

   if (Ahikari <40){
     
     
            digitalWrite(ledArray[0],LOW);
            digitalWrite(ledArray[0],LOW);
     
     
            int HL,HR,HC;
            
          
            while(1){
              
                 int ff=0;
                 
                  mLeft.write(95);
                  mRight.write(95);
                  
                  delay(600);
                  
                  HR = analogRead(hikari);
                  
                  mLeft.write(85);
                  mRight.write(85);
                  delay(600);
                  
                  HC = analogRead(hikari);
                  
                  mLeft.write(85);
                  mRight.write(85);
                  delay(600);
                  HL = analogRead(hikari);
                  
                  mLeft.write(95);
                  mRight.write(95);
                  
                  delay(600);
                  
                  Serial.print(String(HL)+" "+ 
                  String(HC)+" "+
                  String(HR)+" "+"\n" );
                  
                  if(HC < HR || HC < HL){
                    
                    if(HL < HR){
                      
                         mLeft.write(95);
                         mRight.write(95);
                         
                         delay(300);
                      
                    }else{
                      
                         mLeft.write(85);
                         mRight.write(85);
                         
                         delay(300);
                    }
                    
              
                  }
                  
                  for(int i=0;i<5;i++){
                   
                          mLeft.write(90+mspeed);
                          mRight.write(90-mspeed-1);
                          
                          IRLeft1 = readIR(ALeft1);
                          IRLeft2 = readIR(ALeft2);
                          IRRight1 = readIR(ARight1);
                          IRRight2 = readIR(ARight2);
                          IRCenterLeft = readIR(ACenterLeft);
                          IRCenterRight = readIR(ACenterRight);
                          Ahikari = analogRead(hikari);
                          delay(10);
                          
                          if(IRLeft2 > AmaxSide2 || IRLeft1 > AmaxSide1 || IRCenterLeft > AmaxCenter || IRCenterRight > AmaxCenter || IRRight1 > AmaxSide1 || IRRight2 > AmaxSide2){
                            ff=1;
                            //mLeft.write(90);
                            //mRight.write(90);
                            break;
  
                          }
                   
                  }
                  
               if(ff==1){
                  break;
               }   
         
         }
      
    }else{


              
             if(lastdirection==0){
                    
                     digitalWrite(LEDLeft, HIGH);
                     digitalWrite(LEDCenter, LOW);
                     digitalWrite(LEDRight,LOW);
                     
                     mLeft.write(90+mspeed);
                     mRight.write(90);
                     
                     //mRight.write(90-mspeed+turnspeedSide2);
                     //if(IRLeft2 > AmaxSide2 + 50){mRight.write(90-mspeed+turnspeedSide2+2);}
                     //lastdirection=0;
          
               }
                   
                   
               if (lastdirection==1 ){
                     digitalWrite(LEDRight, HIGH);
                     digitalWrite(LEDLeft, LOW);
                     digitalWrite(LEDCenter, LOW);
                     
                     mLeft.write(90);
                     mRight.write(90-mspeed-1);
                     
                     //mLeft.write(90+mspeed-turnspeedSide2);
                     //if(IRRight2 > AmaxSide2 +100){mLeft.write(90+mspeed-turnspeedSide2-2);}
                     //lastdirection=1;
           
                }
              //lastdirection=-1;
              return;     
    } 
    
 
  }
  
  
  if(IRLeft2 > AmaxSide2 && IRLeft1 > AmaxSide1  && IRRight2 < AmaxSide2){
    
    
     if(Ttimes <= 1){
        Ttimes++;
        return;
        
      
      }
      
      Ttimes=0;

      //startF=0;
    
        digitalWrite(LEDLeft, HIGH);
        digitalWrite(LEDCenter, LOW);
        digitalWrite(LEDRight,LOW);
           
        mLeft.write(90+mspeed);
        mRight.write(90);
        
        delay(300);
        lastdirection=0;
        
      
        return;
      
  }
  
  
  
  
   if(IRRight2 > AmaxSide2 && IRRight1 > AmaxSide1 && IRLeft2 < AmaxSide2){
    
     
      if(Ttimes <= 1){
        Ttimes++;
        
      
      }else{
      
     //startF=0;
      
      Ttimes=0;
    
        digitalWrite(LEDLeft, LOW);
        digitalWrite(LEDCenter, LOW);
        digitalWrite(LEDRight,HIGH);
           
        mLeft.write(90);
        mRight.write(90-mspeed-1);
        
        delay(300);
        lastdirection=1;
        
        
        return;
        
      }
  }
  
  
  
   if(IRCenterLeft > AmaxCenter){
          
           digitalWrite(LEDLeft, HIGH);
           digitalWrite(LEDCenter, LOW);
           digitalWrite(LEDRight,LOW);
           
           mRight.write(90-mspeed+turnspeedCenter);
           if(IRCenterLeft>AmaxCenter+300){
             mRight.write(90-mspeed+turnspeedCenter+5);
           }
           
           mLeft.write(90+mspeed);
           //lastdirection=0;
           return;
           
    }else{ 
    
                
         if (IRCenterRight > AmaxCenter ){
            digitalWrite(LEDRight, HIGH);
            digitalWrite(LEDLeft, LOW);
            digitalWrite(LEDCenter, LOW);
            mLeft.write(90+mspeed-turnspeedCenter);
            if(IRCenterRight>AmaxCenter+300){
              mLeft.write(90+mspeed-turnspeedCenter-5);
            }
            mRight.write(90-mspeed-1);
            //lastdirection=1;
            return;
            
          }else{
            
               
          
                mLeft.write(90+mspeed);
                mRight.write(90-mspeed-1);
                
               digitalWrite(LEDRight, LOW);
               digitalWrite(LEDLeft, LOW);
               digitalWrite(LEDCenter, HIGH);
         }
    
    
   }
         
   
  
  
    if(IRLeft1 > AmaxSide1){
          
          digitalWrite(LEDLeft, HIGH);
          digitalWrite(LEDCenter, LOW);
          digitalWrite(LEDRight,LOW);
          mLeft.write(90+mspeed);
          mRight.write(90-mspeed+turnspeedSide1);
          //lastdirection=0;
          return;
     }else{ 
       
       if (IRRight1 > AmaxSide1 ){
          digitalWrite(LEDRight, HIGH);
          digitalWrite(LEDLeft, LOW);
          digitalWrite(LEDCenter, LOW);
          mLeft.write(90+mspeed-turnspeedSide1);
          mRight.write(90-mspeed-1);
          //lastdirection=1;
          return;
          
        }else{
          
            mLeft.write(90+mspeed);
            mRight.write(90-mspeed-1);
            
            digitalWrite(LEDRight, LOW);
            digitalWrite(LEDLeft, LOW);
            digitalWrite(LEDCenter, HIGH);
        }
         
    } 
     
     
     
    if(IRLeft2 > AmaxSide2){
          
           digitalWrite(LEDLeft, HIGH);
           digitalWrite(LEDCenter, LOW);
           digitalWrite(LEDRight,LOW);
           mLeft.write(90+mspeed);
           mRight.write(90-mspeed+turnspeedSide2);
           if(IRLeft2 > AmaxSide2 + 50){mRight.write(90-mspeed+turnspeedSide2+2);}
           lastdirection=0;
           return;
           
     }else{ 
       
       
       if (IRRight2 > AmaxSide2 ){
           digitalWrite(LEDRight, HIGH);
           digitalWrite(LEDLeft, LOW);
           digitalWrite(LEDCenter, LOW);
           mLeft.write(90+mspeed-turnspeedSide2);
           mRight.write(90-mspeed-1);
           if(IRRight2 > AmaxSide2 +100){mLeft.write(90+mspeed-turnspeedSide2-2);}
           lastdirection=1;
           return;
           
       }else{
      
          mRight.write(90-mspeed-1);
          mLeft.write(90+mspeed);
          
          digitalWrite(LEDRight, LOW);
          digitalWrite(LEDLeft, LOW);
          digitalWrite(LEDCenter, HIGH);
       }
       
     }
     
  


}



